#include "cg_remote_control/udp_backend.hpp"
#include <iostream>
#include <string>

#include <ros/ros.h>

using namespace CG;

std::string CodeBuffer(unsigned char *buffer, ssize_t len)
{
    std::string str;
    char byte[4];
    for (int i = 0; i < len; i++)
    {
        sprintf(byte, "%02X ", buffer[i]);
        str += byte;
    }
    return str;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "udp_receiver");
    ros::NodeHandle nh("~");

    int port;
    std::string ip;
    if (!(nh.getParam("port", port) && nh.getParam("ip", ip)))
    {
        ROS_ERROR("Failed to get parameters");
        return -1;
    }

    auto p = NetEndPoint(ip, port);
    auto p_local = NetEndPoint(port);
    UDPBackEnd end(p_local);

    unsigned char buffer[1024];
    ssize_t len;
    NetEndPoint sender;
    while (ros::ok())
    {
        ros::spinOnce();
        if (end.Receive(buffer, 1024, len, sender))
        {
            ROS_INFO("Received message:");
            //ROS_INFO(CodeBuffer(buffer, len).c_str());
        }
    }
    ros::shutdown();
    return 0;
}